#include "ugv_sdk/songling3_1/songling3_1.hpp"
#include <iostream>
#include <termios.h>
#include <unistd.h>
#include <fcntl.h>


using namespace westonrobot;

int kbhit() ；


int main(int argc, char **argv) {
  std::string device_name;
  int32_t baud_rate = 0;

  if (argc == 2) {
    device_name = {argv[1]};
    std::cout << "Specified CAN: " << device_name << std::endl;
  } else if (argc == 3) {
    device_name = {argv[1]};
    baud_rate = std::stol(argv[2]);
    std::cout << "Specified serial: " << device_name << "@" << baud_rate
              << std::endl;
  } else {
    std::cout << "Usage: app_scout_demo <interface>" << std::endl
              << "Example 1: ./app_scout_demo can0" << std::endl
              << "Example 2: ./app_scout_demo /dev/ttyUSB0 115200" << std::endl;
    return -1;
  }

  ScoutBase scout;
  scout.Connect(device_name, baud_rate);

  scout.EnableCommandedMode();

  size_t count = 100000000;
  for (size_t i = 0; i < count; i++) {
    //std::cout << "Motor:0.2,0" << std::endl;
    //scout.SetMotionCommand(0.2, 0.0); //设定线速度为0.2

    usleep(20000); //挂起两万微秒,20毫秒

    // 获取键盘输入
    if (kbhit()) {
      char c = getchar();

      // 根据键盘输入设置运动命令
      switch (c) {
        case 'w': // 前进
          scout.SetMotionCommand(0.2, 0.0);
          break;
        case 's': // 后退
          scout.SetMotionCommand(-0.2, 0.0);
          break;
        case 'a': // 左转
          scout.SetMotionCommand(0.0, 1.0);
          break;
        case 'd': // 右转
          scout.SetMotionCommand(0.0, -1.0);
          break;
        case 'q': // 停止
          scout.SetMotionCommand(0.0, 0.0);
          break;
        default:
          break;
      }
    }
  }

  return 0;
}



int kbhit() {
  static const int STDIN = 0;
  static bool initialized = false;

  if (!initialized) {
    // 将终端设置为非阻塞模式
    struct termios term;
    tcgetattr(STDIN, &term);
    term.c_lflag &= ~ICANON;
    term.c_cc[VMIN] = 0;
    term.c_cc[VTIME] = 0;
    tcsetattr(STDIN, TCSANOW, &term);
    setbuf(stdin, NULL);
    initialized = true;
  }

  // 检查标准输入是否有数据可读
  int bytesWaiting;
  ioctl(STDIN, FIONREAD, &bytesWaiting);
  return bytesWaiting;
}

